package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.AnalogModule;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Encoder;
//import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Ultrasonic;
//import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

public class RobotTemplate extends SimpleRobot {

    static final boolean UPDATE_DASHBOARD_ON = false;

    Joystick leftStick = new Joystick(1);
    Joystick rightStick = new Joystick(2);
    Joystick stick = new Joystick(3);
    //Ultrasonic ultra = new Ultrasonic(1, 6);
    AnalogChannel speedController = new AnalogChannel(2);
    JoystickButton driveForward = new JoystickButton(rightStick, 5);
    JoystickButton driveBack = new JoystickButton(leftStick, 5);
    //Gyro gyro = new Gyro(5);
    //Encoder rightEncoder = new Encoder(3, 4, false, EncodingType.k4X);
    //Encoder leftEncoder = new Encoder(14, 10, false, EncodingType.k4X);
    Encoder rightEncoder = new Encoder(3, 4);
    Encoder leftEncoder = new Encoder(14, 10);

    Timer robotTime = new Timer();

    // variables for testPID
    //boolean inPIDdrive; //flag to indicate we are in PID drive to ensure Ki & Kp have history -may not be needed as it will settle quickly
    double leftEncoderVal, rightEncoderVal;
    double Lspeed =0;
    double Rspeed =0;
    double setLspeed, setRspeed; //speed setpoints
    double Kp, Kpl, Kpr, Kil, Kir, Kdl, Kdr; //Kx constants for left & right drives
    double Kps, Kis, Kds; //Kx constants for shooter
    double lPIDerrorInt = 0; // used for Integral PID control
    double rPIDerrorInt = 0;
    double sPIDerrorInt = 0;
    double previousLerror, previousRerror, previousSerror;// used for Differential PID control

    // for debugging:
    double a[][]= new double [100][3];
    int aIndex = 0;
    int shaIndex =0;
    double lEncoder, rEncoder;

    int mZeroCnt=0, mNZCnt=0; //motor Zero value counter

    double ml1,ml2,mr1,mr2;//values that are output to motors
    double oldPIDint, maxPIDint, minPIDint;

    
    JoystickButton encoderMovementRight = new JoystickButton(rightStick, 9);
    JoystickButton encoderMovementLeft = new JoystickButton(leftStick, 9);
    boolean flagRight = false, flagLeft = false;
    boolean flagStraightForward = false;
    boolean flagStraightBackward = false;
    JoystickButton fineControl = new JoystickButton(leftStick, 1);
    JoystickButton oneStickDrive = new JoystickButton(leftStick, 2);
    final double distance = 200; //distance is .01 inches
    
    //AxisCamera camera = AxisCamera.getInstance();
    CANJaguar rightMotor1;
    CANJaguar rightMotor2;
    CANJaguar leftMotor1;
    CANJaguar leftMotor2;
    RobotDrive myrobot;
    boolean error;
    DigitalInput limitSwitch = new DigitalInput(1);

    //PIDController leftSpeedPID;

   // CANJaguar rightMotor1, rightMotor2, leftMotor1, leftMotor2;
    public void robotInit() {

        try {
            rightMotor1 = new CANJaguar(11, CANJaguar.ControlMode.kPercentVbus);
            rightMotor2 = new CANJaguar(12, CANJaguar.ControlMode.kPercentVbus); //Change these to id Nums of jaguar!!!!!
            leftMotor1 = new CANJaguar(13, CANJaguar.ControlMode.kPercentVbus);
            leftMotor2 = new CANJaguar(15, CANJaguar.ControlMode.kPercentVbus);

        } catch (CANTimeoutException ex) {
            
            ex.printStackTrace();
        }

        rightMotor1.setExpiration(2);// increase motor timeout from default of 0.1s THESE ARE NOT USED
        rightMotor2.setExpiration(2);
        leftMotor1.setExpiration(2);
        leftMotor2.setExpiration(2);

         myrobot = new RobotDrive(rightMotor1,rightMotor2,leftMotor1, leftMotor2);

             updateDashboard();

        leftEncoder.start();
        rightEncoder.start();
        leftEncoder.setDistancePerPulse(5);//calibrate so that Distance of 100 is 1 inch. This is 5 for testRobot
        rightEncoder.setDistancePerPulse(5);

        robotTime.start();
        /*
        camera.writeResolution(AxisCamera.ResolutionT.k160x120);
        camera.writeBrightness(50);
*/
        

        //leftEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
        //leftSpeedPID = new PIDController(0.0001, 0.0001, 0.0, leftEncoder, leftMotor1);
        /*gyro.reset();
        gyro.setSensitivity(.5);*/
       }
        /*Victor rightMotor1 = new Victor(1);
        Victor rightMotor2 = new Victor(2);
        Victor leftMotor1 = new Victor(6);
        Victor leftMotor2 = new Victor(5);*/




    //Autonomous code goes here
    public void autonomous() {

    }

    //Prints data to Driver Station
    public void printToClassmate(){

       DriverStationLCD driverStation = DriverStationLCD.getInstance();
       driverStation.println(DriverStationLCD.Line.kMain6, 1, "lSpeed : " + Double.toString(Math.floor(Lspeed)));
       //driverStation.println(DriverStationLCD.Line.kMain6, 1, "Limit Switch: " + limitSwitch.get());
       driverStation.println(DriverStationLCD.Line.kUser2, 1, "lErr= " + Double.toString(Math.floor((setLspeed-Lspeed)*10)/10)+ "    ");
       driverStation.println(DriverStationLCD.Line.kUser3, 1, "lIntErr= " + Double.toString(Math.floor(lPIDerrorInt))+"     ");
       //driverStation.println(DriverStationLCD.Line.kUser2, 1, "rs-Axis6: " + Double.toString(rightStick.getRawAxis(6)));
       //driverStation.println(DriverStationLCD.Line.kUser2, 1, "L Encoder: " + Double.toString(leftEncoder.getRate()*(-1)));
       driverStation.println(DriverStationLCD.Line.kUser4, 1, "lMot= " + Double.toString(leftMotor1.get())+ "               ");
       //driverStation.println(DriverStationLCD.Line.kUser3, 1, "Right Encoder: " + Double.toString(rightEncoder.getDistance()));
       driverStation.println(DriverStationLCD.Line.kUser5, 1, "rIntErr= " + Double.toString(Math.floor(rPIDerrorInt))+"     ");
       driverStation.println(DriverStationLCD.Line.kUser6, 1, "rMot= " + Double.toString(rightMotor1.get())+ "               ");
       /*driverStation.println(DriverStationLCD.Line.kUser5, 1, "L M Speed : " + Double.toString(Kp*(setLspeed-Lspeed)));
       driverStation.println(DriverStationLCD.Line.kUser6, 1, "R M Speed : " + Double.toString(Kp*(setRspeed-Rspeed)));*/
       //driverStation.println(DriverStationLCD.Line.kUser4, 1, "Gyro angle: " + Double.toString(gyro.getAngle()));

       driverStation.updateLCD();
    }
     //Prints data to Driver Station

    public void printToClassmate2(){

       DriverStationLCD driverStation = DriverStationLCD.getInstance();
       driverStation.println(DriverStationLCD.Line.kMain6, 1, "lSpeed: " + (shaIndex) + ": " + a[shaIndex][0] + "                          ");
       driverStation.println(DriverStationLCD.Line.kUser2, 1, "mSpeed: " + shaIndex + ": " + a[shaIndex][1]  + "                          ");
       driverStation.println(DriverStationLCD.Line.kUser3, 1, "eSpeed: " + (shaIndex) + ": " + a[shaIndex][2]  + "                          ");
       driverStation.println(DriverStationLCD.Line.kUser4, 1, "lSpeed: " + (shaIndex + 1) + ": " + a[shaIndex + 1][0]  + "                          ");
       driverStation.println(DriverStationLCD.Line.kUser5, 1, "mSpeed: " + (shaIndex + 1) + ": " + a[shaIndex + 1][1] + "                          ");
       driverStation.println(DriverStationLCD.Line.kUser6, 1, "eSpeed: " + (shaIndex + 1) + ": " + a[shaIndex + 1][2] + "                          ");
       


       driverStation.updateLCD();
    }

       public void printToClassmate3(){
    /* int mZeroCnt=0, mNZCnt=0; //motor Zero value counter
    double mValu;// motor input value
    double oldPIDint, maxPIDint, minPIDint; */

       DriverStationLCD driverStation = DriverStationLCD.getInstance();
       driverStation.println(DriverStationLCD.Line.kMain6, 1, "mZeroCnt: " + mZeroCnt + "                          ");
       driverStation.println(DriverStationLCD.Line.kUser2, 1, "mNZCnt: "  + mNZCnt  + "                          ");
       driverStation.println(DriverStationLCD.Line.kUser3, 1, "leftEncoderVal: " + (Math.floor(leftEncoderVal*1)/1) + "                          ");
       driverStation.println(DriverStationLCD.Line.kUser4, 1, "rightEncoderVal: " + (Math.floor(rightEncoderVal*1)/1)  + "                          ");
       driverStation.println(DriverStationLCD.Line.kUser5, 1, "ml1: " + (Math.floor(ml1*100)/100) + "                          ");
       driverStation.println(DriverStationLCD.Line.kUser6, 1, "mr1: " + (Math.floor(mr1*100)/100) + "                          ");



       driverStation.updateLCD();
    }


    //Takes String parameter and outputs to the Driver Station
    public void debug(String a){
       DriverStationLCD driverStation = DriverStationLCD.getInstance();
       driverStation.println(DriverStationLCD.Line.kUser2, 1, a);
       driverStation.updateLCD();
    }
        public void debugLine2(String a){
       DriverStationLCD driverStation = DriverStationLCD.getInstance();
       driverStation.println(DriverStationLCD.Line.kUser3, 1, a);
       driverStation.updateLCD();
    }

    //Copied method updates Dashboard
    public void updateDashboard() {
        Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();
        lowDashData.addCluster();
        {
            lowDashData.addCluster();
            {     //analog modules
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(1).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(2).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
            }
            lowDashData.finalizeCluster();

            lowDashData.addCluster();
            { //digital modules
                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 1;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 2;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayReverse());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

            }
            lowDashData.finalizeCluster();

            lowDashData.addByte(Solenoid.getAllFromDefaultModule());
        }
        lowDashData.finalizeCluster();
        lowDashData.commit();

    }

    //Drives robot, inputs have to be flipped for Tank Drive
    public void drive(){
          if(fineControl.get() == true && oneStickDrive.get() == true){
                //if((leftStick.getY() / 2 < 1) && (leftStick.getY() / 2 > -1)){
                    /*leftMotor1.setX(leftStick.getY() / 2);
                    leftMotor2.setX(leftStick.getY() / 2);
                    rightMotor1.setX(-leftStick.getY() / 2);
                    rightMotor2.setX(-leftStick.getY() / 2);*/
              myrobot.tankDrive(-leftStick.getY()/2, -leftStick.getY()/2);

          }
            else if(fineControl.get() == true && oneStickDrive.get() == false){
                /*leftMotor1.set(leftStick.getY() / 2);
                leftMotor2.set(leftStick.getY() / 2);
                rightMotor1.set(-rightStick.getY() / 2);
                rightMotor2.set(-rightStick.getY() / 2);*/
                myrobot.tankDrive(-rightStick.getY()/2, -leftStick.getY()/2);
            }
            else if(oneStickDrive.get() == true && fineControl.get() == false){
                /*leftMotor1.set(leftStick.getY());
                leftMotor2.set(leftStick.getY());
                rightMotor1.set(-leftStick.getY());
                rightMotor2.set(-leftStick.getY());*/
                myrobot.tankDrive(-leftStick.getY(), -leftStick.getY());
            }
            else{
                /*leftMotor1.set(leftStick.getY());
                leftMotor2.set(leftStick.getY());
                rightMotor1.set(-rightStick.getY());
                rightMotor2.set(-rightStick.getY());*/
            }
           myrobot.tankDrive(-rightStick.getY(), -leftStick.getY());
    }

    //Outputs power to motors using a sine wave. (Used in encoder methods)
    public double getPowerLeft(){
        double d;
        double power = Math.abs((leftStick.getRawAxis(4)-1)/2);
        d = Math.abs(leftEncoder.getDistance());
        if(d > distance){
            power=0;
        }else{
            if (d<distance/2){
                power = 0.5;
            }else{
                power = power * Math.sin(d/distance * Math.PI);
            }
        }
        
       //debug(Double.toString(power));
        return power;
    }

    //Outputs power to motors using a sine wave. (Used in encoder methods)
    public double getPowerRight(){
      double d;
        double power = Math.abs((leftStick.getRawAxis(4)-1)/2);
        d = Math.abs(rightEncoder.getDistance());
        if(d > distance){
            power=0;
        }else{
            if (d<distance/2){
                power = 0.5;
            }else{
                power = power * Math.sin(d/distance * Math.PI);
            }
        }

       //debug(Double.toString(power));
        return power;
    }

    //Turns slightly using encoders
    public void fineMovementTurning(){
        //Button 9 on left stick causes robot to turn to the left
        if(encoderMovementLeft.get() == true && !flagLeft){
                flagLeft = true;
                leftEncoder.reset();
                rightEncoder.reset();
        }
            if(flagLeft == true){
                if(leftEncoder.getDistance() > -distance){
                   leftMotor1.set(getPowerLeft());
                   leftMotor2.set(getPowerLeft());

                }else{
                    leftMotor1.set(0);
                    leftMotor2.set(0);
                }
                if(rightEncoder.getDistance() > -distance){

                   rightMotor1.set(getPowerRight());
                   rightMotor2.set(getPowerRight());
                }else{
                    rightMotor1.set(0);
                    rightMotor2.set(0);
                }
                 if(rightEncoder.getDistance() < -distance && leftEncoder.getDistance() < -distance){
                    flagLeft = false;
                }
            }
           //Button 9 on right stick causes robot to turn to the right
           if(encoderMovementRight.get() == true && !flagRight){
                flagRight = true;
                leftEncoder.reset();
                rightEncoder.reset();
            }
            if(flagRight == true){
                if(leftEncoder.getDistance() < distance){
                    leftMotor1.set(-getPowerLeft());
                    leftMotor2.set(-getPowerLeft());
                }else{
                    leftMotor1.set(0);
                    leftMotor2.set(0);
                }
                if(rightEncoder.getDistance() < distance){
                    rightMotor1.set(-getPowerRight());
                    rightMotor2.set(-getPowerRight());
                }else{
                    rightMotor1.set(0);
                    rightMotor2.set(0);
                }
                if(rightEncoder.getDistance() > distance && leftEncoder.getDistance() > distance){
                    flagRight = false;

                }
            }

    }

    //Goes straight forwar or back lightly using encoders
    public void fineMotorStraight(){
        //Button 5 on right stick move robot forward
        if(driveForward.get() == true && !flagStraightForward){
                flagStraightForward = true;
                leftEncoder.reset();
                rightEncoder.reset();
           }

        if(flagStraightForward == true){
            if(leftEncoder.getDistance() < distance){
                leftMotor1.set(-getPowerLeft());
                leftMotor2.set(-getPowerLeft());
            }else{
                leftMotor1.set(0);
                leftMotor2.set(0);
            }
            if(rightEncoder.getDistance() > -distance){
                rightMotor1.set(getPowerRight());
                rightMotor2.set(getPowerRight());
            }else{
                rightMotor1.set(0);
                rightMotor2.set(0);
            }
            if(rightEncoder.getDistance() < -distance && leftEncoder.getDistance() > distance){
                flagStraightForward = false;
            }
        }

            //Button 5 on left stick moves robot back
           if(driveBack.get() == true && !flagStraightBackward){
                flagStraightBackward = true;
                leftEncoder.reset();
                rightEncoder.reset();
            }
           if(flagStraightBackward == true){
                if(leftEncoder.getDistance() > -distance){
                    leftMotor1.set(getPowerLeft());
                    leftMotor2.set(getPowerLeft());
                }else{
                    leftMotor1.set(0);
                    leftMotor2.set(0);
                }
                if(rightEncoder.getDistance() < distance){
                    rightMotor1.set(-getPowerRight());
                    rightMotor2.set(-getPowerRight());
                }else{
                    rightMotor1.set(0);
                    rightMotor2.set(0);
                }
                if(rightEncoder.getDistance() > distance && leftEncoder.getDistance() < -distance){
                    flagStraightBackward = false;
                }
            }



    }

     //Main PID drive L, R, REV, FWD
     public void testPIDFine(int a) {
        // a=1 Left turn
         // 2 Right turn
         // 3 Reverse
         // 4 Forward

         final double MAX_MOTOR_POWER = 0.5;// cap the max value to avoid erratic trashing
 
        int leftFWD =1 ;
        int rightFWD =1 ;

        

        // Init stuff for testPID
        //setLspeed=(leftStick.getRawAxis(4)+1)*5000+1; //SetPoint
        setLspeed=(leftStick.getRawAxis(4)+1)*1000+1; //SetPoint
        
        //setLspeed = 900;
        setRspeed=setLspeed;


        Kp= 0.25 * 1/setLspeed;//set the first number 0-1 for a 0-100% factor.The second number is 1/maxSpeed to normalise Kp.
        Kil= 0.05 * 1/setLspeed;//set the first number 0-1 for a 0-100% factor.The second number is 1/maxSpeed to normalise K.
        Kir= 0.05 * 1/setRspeed;
        Kdl= 0.00 * 1/setLspeed;
        Kdr= 0.00 * 1/setRspeed;
        //setLspeed = setLspeed * 2;
        // eo Init stuff for testPID

        if(a == 1){//Left Turn
            leftFWD = -1;
            rightFWD = 1;
        }
        if (a==2){//Right Turn
            leftFWD = 1;
            rightFWD = -1;
        }
        if(a == 3){//Reverse
            leftFWD = -1;
            rightFWD = -1;
            //setLspeed *= 2;
            //setRspeed *= 2;
        }
        if(a==4){//Forward
            leftFWD = 1;
            rightFWD = 1;
            //setLspeed *= 2;
            //setRspeed *= 2;
        }

         //this code can be inside the "porous" finemotion when a button is pressed.
        leftEncoderVal=leftEncoder.getRate()* leftFWD;
        rightEncoderVal=rightEncoder.getRate() * rightFWD * -1;

        if(Double.isNaN(Lspeed)){ //for some reason the encoder object returns NaN, so just overwrite it!!!
            Lspeed = 0;
            error = true;
        }
        if(Double.isNaN(Rspeed)){
            Rspeed = 0;
            error = true;
        }

        //Lspeed=(Lspeed + leftEncoderVal)/2; // this is done to smooth getRate as it is naturally erratic.
        //Rspeed=(Rspeed + rightEncoderVal)/2;
        Lspeed=leftEncoderVal;// it seems more unstable without the averaging above
        Rspeed=rightEncoderVal;

        ml1=Kp*(setLspeed-Lspeed)+Kil*lPIDerrorInt+Kdl*((setLspeed-Lspeed)-previousLerror);
        mr1=Kp*(setRspeed-Rspeed)+Kir*rPIDerrorInt+Kdr*((setRspeed-Rspeed)-previousRerror);
        if (ml1>MAX_MOTOR_POWER) ml1=MAX_MOTOR_POWER;// cap the max value to avoid erratic trashing
        if (mr1>MAX_MOTOR_POWER) mr1=MAX_MOTOR_POWER;
        ml2=ml1;
        mr2=mr1;


        if (aIndex<100) { // the a array is purely for debugging to capture & display parameters
            //a[aIndex][0]=Lspeed;
            //a[aIndex][1]=Kp*(setLspeed-Lspeed);
            //a[aIndex++][2]=encoderVal;
           /* try {
                leftMotor1.setX(ml1 * leftFWD * -1);
                leftMotor2.setX(ml2 * leftFWD * -1);
                rightMotor1.setX(mr1 * rightFWD);
                rightMotor2.setX(mr2 * rightFWD);
             } catch (CANTimeoutException ex) {

            ex.printStackTrace();
            }*/
            leftMotor1.set(ml1 * leftFWD * -1);
            leftMotor2.set(ml2 * leftFWD * -1);
            rightMotor1.set(mr1 * rightFWD);
            rightMotor2.set(mr2 * rightFWD);

            //myrobot.tankDrive(ml1 * leftFWD * -1, mr1 * rightFWD);

        } else {
            try {
                leftMotor1.setX(0);
                leftMotor2.setX(0);
                rightMotor1.setX(0);
                rightMotor2.setX(0);
              } catch (CANTimeoutException ex) {

            ex.printStackTrace();
            }
           
        }
       
        oldPIDint=lPIDerrorInt;
        lPIDerrorInt=lPIDerrorInt + (setLspeed-Lspeed); //compute integral of PID error
        rPIDerrorInt=rPIDerrorInt + (setRspeed-Rspeed); //compute integral of PID error
        if (lPIDerrorInt>maxPIDint) maxPIDint=lPIDerrorInt;
        if (lPIDerrorInt<minPIDint) minPIDint=lPIDerrorInt;

        previousLerror=setLspeed-Lspeed;// differential PID
        previousRerror=setRspeed-Rspeed;

        if (ml1==0) mZeroCnt++; else mNZCnt++;

        //Timer.delay(0.01);
        //Rmotor.set(Kp*(setRspeed-Rspeed));
    }
    
    public void testPIDShooter() {
        double encoderVal;

        // Init stuff for testPID
        setLspeed=(leftStick.getRawAxis(4)+1)*5000; //SetPoint
        //setRspeed=setLspeed;

        Kp= 1 * 1/setLspeed;//set the first number 0-1 for a 0-100% factor.The second number is 1/maxSpeed to normalise Kp.
        setLspeed = setLspeed * 2;
        // eo Init stuff for testPID
        
         //this code can be inside the "porous" finemotion when a button is pressed.
        encoderVal=leftEncoder.getRate()*(-1);  // this is done to smooth getRate as it is naturally erratic.
        Lspeed=(Lspeed + encoderVal)/2;
        //Rspeed=(Rspeed + rightEncoder.getRate())/2;

        // The P in PID:
        if (aIndex<100) {
            //a[aIndex][0]=Lspeed;
            //a[aIndex][1]=Kp*(setLspeed-Lspeed);
            //a[aIndex++][2]=encoderVal;
            leftMotor1.set(Kp*(setLspeed-Lspeed));
            leftMotor2.set(Kp*(setLspeed-Lspeed));
        } else {
            leftMotor1.set(0);
            leftMotor2.set(0);
        }

        //Timer.delay(0.01);
        //Rmotor.set(Kp*(setRspeed-Rspeed));
    }

    public void operatorControl() {


            //leftSpeedPID.setTolerance(0.50);
            //leftSpeedPID.setSetpoint(2000);
            //leftSpeedPID.setContinuous();
            //leftSpeedPID.enable();
            //leftSpeedPID.setInputRange(-5000, 5000);

        while (isOperatorControl() && isEnabled())
        {

            if(UPDATE_DASHBOARD_ON){
                updateDashboard();
            }

        //    drive();

            //fineMovementTurning();

           /* Code to operate the bridge arm:
            if(leftStick.getRawButton(11)){
                leftMotor2.set(-0.2);
            }
            if(leftStick.getRawButton(12)){
                leftMotor2.set(0.2);
            } */


            /*if(rightStick.getRawAxis(5) > 0.5){
                testPIDFine(1);
            }
            if(rightStick.getRawAxis(5) < -0.5){
                testPIDFine(2);
            }*/
            if(rightStick.getRawAxis(6) > 0.5){// REV PID
                testPIDFine(3);
            }
            if(rightStick.getRawAxis(6) < -0.5){// FWD PID
                testPIDFine(4);
            }
            if(rightStick.getRawButton(2)){
                if (rightStick.getRawAxis(3) > 0.5) testPIDFine(2);// Right turn PID
                if (rightStick.getRawAxis(3) < -0.5) testPIDFine(1);// Left turn PID
            }

            printToClassmate3();
           
          
            if (leftStick.getRawButton(10)){
                updateDashboard();
                printToClassmate3();
            }

             if (leftStick.getRawButton(9)){
              
                printToClassmate3();
            }


             if (rightStick.getRawButton(4)) {
                myrobot.tankDrive(-rightStick.getRawAxis(3) / 1.7, rightStick.getRawAxis(3) / 1.7);
             }


            //fineMotorStraight();


        }
    }
}